% FILE: uav_sys_id.m
% DESCRIPTION:This demo file is used for frequency domain system
% identification using tfestimate() method.

clc,clear,close all

PLOT = 1;

%% load data
file_name = 'ulog_rate_chirp_sweep_result.mat';
load(file_name);

% sampling Time and freq
ts = median(diff(t));
fs = 1/ts;

%% set options
% freq range to be shown in frequency response
freq_show_range = [0.01 50];

% freq range to be fited or identified
freq_id_frd = [2 10]; % choose the freq range with high coherence, i.e, greater than 90%.

%% tfestimate options
freq_point = fs;
window_size = 4*fs; % 2-4 times sample frequnecies
overlap = 2;

tfest_opt = tfestOptions();
tfest_opt.EnforceStability = 1;

% model poles (order)
Np = 2;
% model zeros
Nz = 1;
% model delay
io_delay = 0*ts;

%% bode plot options
op = bodeoptions();
op.FreqUnits = 'Hz';
op.XLim = freq_show_range;
op.PhaseMatching = 'on';
op.PhaseVisible = 'on';
op.PhaseUnits = 'deg';
op.FreqScale = 'log';
op.Grid = 'on';

%% system identification
freq_range = logspace(log10(freq_show_range(1)), log10(freq_show_range(2)), freq_point);
freq_id_range = logspace(log10(freq_id_frd(1)), log10(freq_id_frd(2)), freq_point);

window = hanning(window_size);
noverlap = round(window_size/overlap);
[T_uy, F_uy] = tfestimate(u, y, window, noverlap, freq_range, fs);
[C_uy, ~] = mscohere(u, y, window, noverlap, freq_range, fs);
plant_frd = frd(T_uy, F_uy*2*pi, 1/fs);

[T_uy_id, F_uy_id] = tfestimate(u, y, window, noverlap, freq_id_range, fs);
[C_uy_id, ~] = mscohere(u, y, window, noverlap, freq_id_range, fs);
plant_frd_id = frd(T_uy_id, F_uy_id*2*pi, 1/fs);

% estimate transfer function using tfest().
plant_id = tfest(plant_frd_id, Np, Nz, io_delay, tfest_opt)

%% plot results (optional)
if PLOT
    Fig_1 = figure(1);
    Fig_1.Name = 'Frd response';
    
    subplot(3,1,1);
    semilogx(F_uy, mag2db(abs(T_uy)));
    hold on;
    semilogx(F_uy_id, mag2db(abs(T_uy_id)));
    legend('Gain Response');
    ylabel('Gain/dB');
    grid on;
    
    subplot(3,1,2);
    semilogx(F_uy, phase(T_uy) * 180 / pi);
    hold on;
    semilogx(F_uy_id, phase(T_uy_id) * 180 / pi);
    ylabel('Phase/deg');
    legend('Phase response');
    grid on;
    
    subplot(3,1,3);
    semilogx(F_uy, C_uy);
    hold on;
    semilogx(F_uy_id, C_uy_id);
    ylabel('Coherence');
    xlabel('freq/Hz');
    legend('Coherence');
    grid on;

    Fig_2 = figure(2);
    Fig_2.Name = 'identified plant with frd response';
    
    bode(plant_frd, plant_frd_id, plant_id, op);
    legend('frd', 'frd_{id}', 'plant_{id}');

    %examinate identified plant
    t_interpl = (t(1):ts:t(end))';
    u_interpl = interp1(t, u, t_interpl);
    y_id1 = lsim(plant_id, u_interpl, t_interpl);
    
    Fig_3 = figure(3);
    Fig_3.Name = 'Time-domain Examination';
    plot(t, y);
    hold on;
    plot(t_interpl, y_id1,'--');
    legend('y', 'y_{id1}');
end;


